#include "KayaoPhysics.h"
#include "box2d/Include/Box2D.h"
#include "stdlib.h"
#include "stdio.h"
#include <jni.h>

#define MAX_BODIES 300
#define VELOCITY_ITERATIONS 8
#define POSITION_ITERATIONS 2
#define ALLOW_SLEEP false
#define GRAVITY_X 0
#define GRAVITY_Y 0
#define FRICTION_BOX_STATIC 0.1f
#define LINEAR_DAMPING 10.0f
#define ANGULAR_DAMPING 10.0f

b2Body *bodies[MAX_BODIES];
b2World *world = NULL;

b2Body *bodyDR;
b2World *worldDR = NULL;

/*
 * Returns the index of a given body.
 */
int32 getIdOfBody(b2Body *body) {
	for(int32 i = 0; i < MAX_BODIES; i++) {
		if(bodies[i] == body) {
			return i;
		}
	}
	return -1;
}

/*
 * Returns a free index in the bodies array.
 */
int32 getFreeBodyId() {
	for(int32 i = 0; i < MAX_BODIES; i++) {
		if(bodies[i] == NULL) {
			return i;
		}
	}
	return -1;
}

void initializeDRBody() {
	b2BodyDef groundBodyDef;
	groundBodyDef.position.Set(0, 0);

	b2Body* groundBody = world->CreateBody(&groundBodyDef);

	// Define the ground box shape.
	b2PolygonDef groundShapeDef;

	// The extents are the half-widths of the box.
	groundShapeDef.SetAsBox(1 / 2, 1 / 2);

	groundShapeDef.density = 0.1f;
	groundShapeDef.friction = 0.1f;
	groundShapeDef.restitution = 0.1f;

	// Add the ground shape to the ground body.
	groundBody->CreateFixture(&groundShapeDef);
	groundBody->SetMassFromShapes();
	bodyDR = groundBody;
}

/*
 * Class:     kayao_client_scene_PhysicsController
 * Method:    step
 * Signature: (F)V
 */
JNIEXPORT void JNICALL Java_kayao_client_scene_PhysicsController_step(JNIEnv *env, jclass caller, jfloat timeStep) {
	world->Step(timeStep, VELOCITY_ITERATIONS, POSITION_ITERATIONS);
}

/*
 * Class:     kayao_client_scene_PhysicsController
 * Method:    updateBody
 * Signature: (Lkayao/client/scene/objects/PhysicalNode;I)V
 */
JNIEXPORT void JNICALL Java_kayao_client_scene_PhysicsController_updateBody(JNIEnv *env, jclass caller, jobject jBody, jint id) {
	if (bodies[id] != NULL) {
		jclass jBodyClass = env->GetObjectClass(jBody);
		jmethodID setValuesId = env->GetMethodID(jBodyClass, "setValues", "(FFFFF)V");
		b2Body *body = bodies[id];
		env->CallVoidMethod(jBody, setValuesId, body->GetPosition().x, body->GetPosition().y, body->GetAngle(), body->GetLinearVelocity().x, body->GetLinearVelocity().y);
	}
}

/*
 * Class:     kayao_client_scene_PhysicsController
 * Method:    setCarValues
 * Signature: (IFFFFFF)V
 */
JNIEXPORT void JNICALL Java_kayao_client_scene_PhysicsController_setCarValues(JNIEnv *env, jclass caller, jint id, jfloat positionX, jfloat positionY, jfloat rotation, jfloat angularVelocity, jfloat velocityX, jfloat velocityY) {
	if (bodies[id] != NULL) {
		b2Body *body = bodies[id];
		body->SetXForm(b2Vec2(positionX, positionY), rotation);
		body->SetAngularVelocity(angularVelocity);
		body->SetLinearVelocity(b2Vec2(velocityX, velocityY));
	}
}

/*
 * Class:     kayao_client_scene_PhysicsController
 * Method:    createWorld
 * Signature: (FFFF)V
 */
JNIEXPORT void JNICALL Java_kayao_client_scene_PhysicsController_createWorld(JNIEnv *env, jclass caller, jfloat minX, jfloat minY, jfloat maxX, jfloat maxY) {
	// Clear if needed
	if(world != NULL) {
		for(int32 i = 0; i < MAX_BODIES; i++) {
			if(bodies[i] != NULL) {
				world->DestroyBody(bodies[i]);
				bodies[i] = NULL;
			}
		}
		world = NULL;
	}

	b2AABB worldAABB;
	worldAABB.lowerBound.Set(minX, minY);
	worldAABB.upperBound.Set(maxX, maxY);

	// Create the gravity vector.
	b2Vec2 gravity(GRAVITY_X, GRAVITY_Y);

	// Construct a world object, which will hold and simulate the rigid bodies.
	world = new b2World(worldAABB, gravity, ALLOW_SLEEP);

	//******************************************
	// Create seperate world for dead Reckoning
    //******************************************
	// Clear if needed
	if(worldDR != NULL) {
		if(bodyDR != NULL) {
			worldDR->DestroyBody(bodyDR);
			bodyDR = NULL;
		}
		world = NULL;
	}

	b2AABB worldAABBDR;
	worldAABBDR.lowerBound.Set(minX, minY);
	worldAABBDR.upperBound.Set(maxX, maxY);

	// Create the gravity vector.
	b2Vec2 gravityDR(GRAVITY_X, GRAVITY_Y);

	// Construct a world object, which will hold and simulate the rigid bodies.
	worldDR = new b2World(worldAABBDR, gravityDR, ALLOW_SLEEP);
	initializeDRBody();
}

/*
 * Class:     kayao_client_scene_PhysicsController
 * Method:    createDynamicBox
 * Signature: (FFFFFFF)I
 */
JNIEXPORT jint JNICALL Java_kayao_client_scene_PhysicsController_createDynamicBox(JNIEnv *env, jclass caller, jfloat x, jfloat y, jfloat width, jfloat height, jfloat density, jfloat restitution, jfloat friction) {
	int32 id = getFreeBodyId();

	b2BodyDef groundBodyDef;
	groundBodyDef.position.Set(x, y);
	//groundBodyDef.linearDamping = LINEAR_DAMPING;
	//groundBodyDef.angularDamping = ANGULAR_DAMPING;

	b2Body* groundBody = world->CreateBody(&groundBodyDef);

	// Define the ground box shape.
	b2PolygonDef groundShapeDef;

	// The extents are the half-widths of the box.
	groundShapeDef.SetAsBox(width / 2, height / 2);

	groundShapeDef.density = density;
	groundShapeDef.friction = restitution;
	groundShapeDef.restitution = friction;

	// Add the ground shape to the ground body.
	groundBody->CreateFixture(&groundShapeDef);
	groundBody->SetMassFromShapes();
	bodies[id] = groundBody;

	return id;
}

/*
 * Class:     kayao_client_scene_PhysicsController
 * Method:    createStaticBox
 * Signature: (FFFF)I
 */
JNIEXPORT jint JNICALL Java_kayao_client_scene_PhysicsController_createStaticBox(JNIEnv *env, jclass caller, jfloat x, jfloat y, jfloat width, jfloat height) {
	int32 id = getFreeBodyId();

	b2BodyDef groundBodyDef;
	groundBodyDef.position.Set(x, y);

	b2Body* groundBody = world->CreateBody(&groundBodyDef);

	// Define the ground box shape.
	b2PolygonDef groundShapeDef;

	// The extents are the half-widths of the box.
	groundShapeDef.SetAsBox(width / 2, height / 2);

	groundShapeDef.density = 0.0f;
	groundShapeDef.friction = FRICTION_BOX_STATIC;
	groundShapeDef.restitution = 0.0f;

	// Add the ground shape to the ground body.
	groundBody->CreateFixture(&groundShapeDef);
	groundBody->SetMassFromShapes();
	bodies[id] = groundBody;

	return id;
}

/*
 * Class:     kayao_client_scene_PhysicsController
 * Method:    destroyBody
 * Signature: (I)V
 */
JNIEXPORT void JNICALL Java_kayao_client_scene_PhysicsController_destroyBody(JNIEnv *env, jclass caller, jint id) {
	if(bodies[id] != NULL) {
		world->DestroyBody(bodies[id]);
		bodies[id] = NULL;
	}
}

/*
 * Class:     kayao_client_scene_PhysicsController
 * Method:    performDeadReckoning
 * Signature: (Lkayao/client/scene/objects/PhysicalNode;FFFFFFF)V
 */
JNIEXPORT void JNICALL Java_kayao_client_scene_PhysicsController_performDeadReckoning(JNIEnv *env, jclass caller, jobject jBody, jfloat positionX, jfloat positionY, jfloat rotation, jfloat angularVelocity, jfloat velocityX, jfloat velocityY, jfloat timeStep) {
	if (bodyDR != NULL) {
		b2Body *body = bodyDR;
		body->SetXForm(b2Vec2(positionX, positionY), rotation);
		body->SetAngularVelocity(angularVelocity);
		body->SetLinearVelocity(b2Vec2(velocityX, velocityY));

		//for(int32 i = 0; i < timeStep / 0.2f; i++) {
		//	world->Step(0.2f, VELOCITY_ITERATIONS, POSITION_ITERATIONS);
		//}
		world->Step(0.025f, VELOCITY_ITERATIONS, POSITION_ITERATIONS);
	}

	if (bodyDR != NULL) {
		b2Body *body = bodyDR;
		jclass jBodyClass = env->GetObjectClass(jBody);
		jmethodID setValuesId = env->GetMethodID(jBodyClass, "setValues", "(FFFFF)V");
		env->CallVoidMethod(jBody, setValuesId, body->GetPosition().x, body->GetPosition().y, body->GetAngle(), body->GetLinearVelocity().x, body->GetLinearVelocity().y);
	}
}
